/*LICENSE_BEGIN
 * ******************************************************************
 * Copyright 2006 by the Dainamite Robocup-Team at the
 * DAI-Labor, TU-Berlin (www.dai-labor.de)
 * 
 * Members of Dainamite are:
 * Holger Endert
 * Robert Wetzker
 * Thomas Karbe
 * Axel Heßler
 * Philippe Bütter
 * Felix Brossmann
 * 
 * Licensed under the GNU GPL (General Public License) Free Software 
 * License, Version 2. You may use this file only in compliance with 
 * the License. You may obtain a copy of the License at:
 * 
 * http://www.fsf.org/licensing/licenses/gpl.txt
 * ******************************************************************
LICENSE_END*/
package robocup.component.worldmodel;

import static java.lang.Math.signum;
import robocup.component.SConf;
import robocup.component.WorldModel;
import robocup.component.ControllerNumbers.PLAY_MODE;
import robocup.component.geometry.Vektor;
import robocup.component.infotypes.PlayModeInfo;
import robocup.component.infotypes.PlayerSayInfo;
import robocup.component.infotypes.RefereeInfo;
import robocup.component.infotypes.SenseBodyInfo;
import robocup.component.infotypes.VisualInfo;
import robocup.component.worldobjects.Ball;
import robocup.component.worldobjects.Player;
import robocup.component.worldobjects.Player.UPDATE_MODE;

/**
 * The {@link CollisionModel} provides method for correcting previously done
 * updates in the {@link MeModel} and {@link BallModel} when a collision with 
 * the Ball occured.
 * If two objects collide the speed of each objects is multiplied by -0.1 and
 * they are set outside each other so that you will never find 2 objects over-
 * laping (at least in PLAY_MODE==PLAY_ON). Most of the times it is easy to 
 * tell wether we collided as our speed differs significantly from the expected 
 * speed. However, there remain 2 cases where a collision with the ball is hard 
 * to detect:
 * 
 *  1. Another player is near and it is not clear wether we collided with the 
 *  ball or with this player. Using the information contained in a visual info
 *  might help in this case if the ball speed is explicitly seen or the ball
 *  and/or other player is too far for a collision.
 *  
 *  2. If we are very slow it is hard to tell wether our speed was multiplied 
 *  by -0.1 or if this effect was caused by noise. Again, using the information 
 *  contained in the visual info might help here.
 *  
 */
public class CollisionModel implements IModel {

	private WorldModel world;

  //true if we collided with something
	private boolean collided = true;

  //true if that something was the ball
	private boolean collidedWithBall = false;

  //true if another player might be the object we collided with
	private boolean mightHaveCollidedWithPlayer = false;

  //the posiiton we were supposed to be
	private Vektor expectedPosition = Vektor.getDummyInstance();

	/**
	 * Constructor - a model to detect collisions with the ball.
   * 
	 * @param world -
	 *            the current world-model
	 */
	public CollisionModel(WorldModel world) {

		this.world = world;
	}

	/**
	 * resets all cycle data.
	 * 
	 */
	private void reset() {

		this.collided = false;
		this.collidedWithBall = false;
		this.mightHaveCollidedWithPlayer = false;
	}

	/**
	 * updating model after BS.
	 * 
	 * @param info
	 */
	public void update(SenseBodyInfo info) {

		this.reset();

		// only play_on
		if (this.world.getPlayMode() != PLAY_MODE.PLAY_ON) {

			// if we collided with the ball in an dead cycle, the server doesn't
			// move the ball outside the player here the ball is set outside the 
      // player preventing strange method results
			if (this.world.getSelfRef().getDistance(this.world.getBallRef()) 
          < this.world.getSelfRef().getPConf().PLAYER_SIZE
					+ SConf.getInstance().BALL_SIZE) {
				Ball ballRef = this.world.getBallRef();
				double angle = this.world.getSelfRef().getAngleTo(
						ballRef.getPosition());

				this.world.getSelfRef().getPosition().pointAtPolar(
						this.world.getMyPConf().PLAYER_SIZE
								+ SConf.getInstance().BALL_SIZE + 0.001, angle);
				this.world.getSelfRef().getPosition().subFromThis(
						this.world.getBallRef().getPosition());
				this.world.getSelfRef().getPosition().mult(-1);

				this.world.getExpectedPlayers()[this.world.getNumber() - 1]
						.copy(this.world.getSelfRef());
				this.world.getNextPlayers()[this.world.getNumber() - 1]
						.copy(this.world.getSelfRef());
			}
			return;
		}

		// as we collided the speed given in the BS is not the speed used by the
		// server for calculating our movement. for checking for collision we have 
    // to undo this effect
		this.expectedPosition.copy(this.world.getSelfRef().getPosition());

		// collided
		if (this.checkIfCollided()) {
			this.collided = true;

			// calculate position as it would have been without collision
			this.expectedPosition.subFromThis(this.world.getSelfRef().getSpeed()
							.mult2(11 / this.world.getSelfRef().getPConf().PLAYER_DECAY));
			this.world.getSelfRef().setPosition(this.expectedPosition);

			this.world.getExpectedPlayers()[this.world.getNumber() - 1]
					.copy(this.world.getSelfRef());
			this.world.getNextPlayers()[this.world.getNumber() - 1]
					.copy(this.world.getSelfRef());

			// test if it is possible that we collided with someone (used here
			// for setting security margins)
			this.mightHaveCollidedWithPlayer = this
					.checkIfMightHaveCollidedWithPlayer();
		}

		// collided with ball
		if (this.checkIfCollidedWithBallAfterBS()) {
			this.collided = true;
			this.collidedWithBall = true;

			// collision with ball has nearly no effect on the players last
			// movement
			this.world.getSelfRef().setPosition(this.expectedPosition);

			this.world.getExpectedPlayers()[this.world.getNumber() - 1]
					.copy(this.world.getSelfRef());
			this.world.getNextPlayers()[this.world.getNumber() - 1]
					.copy(this.world.getSelfRef());

			// the ball can't be inside the player, instead the server moves it
			// outside
			this.moveCollidedBallToPlayerMargin();

			// "stop" ball
			this.world.getBallRef().getSpeed().mult(-0.1);
			this.world.getBallRef().setSpeedReliable(false);
		} else {
			this.collidedWithBall = false;
		}

		// if the ball was predicted inside the player remove this effect
		// (as some methods go mad if the ball is inside)
		if (this.world.getSelfRef().getDistance(this.world.getBallRef()) < this.world
				.getSelfRef().getPConf().PLAYER_SIZE
				+ SConf.getInstance().BALL_SIZE) {
			this.moveCollidedBallToPlayerMargin();
		}

	}

	/**
	 * updating model after VI.
	 * 
	 * @param info
	 */
	public void update(VisualInfo info) {

		// collided with ball 
    // if the speed is not seen the ballSpeed is calculated using current
		// and last known ball position (ballModel). if we collided with the ball
		// this has to be corrected
		if (this.checkIfCollidedWithBallAfterVI()) {
			this.world.getBallRef().getSpeed().mult(-0.1);
			this.world.getBallRef().setSpeedReliable(false);
			this.collided = true;
			this.collidedWithBall = true;
			this.world.resetFilterAfterCollisionWithBall();
		} else {
			this.collidedWithBall = false;
		}

		// if the ball was seen inside the player due to noise remove this
		// effect (as some methods go mad if the ball is inside)
		if (this.world.getSelfRef().getDistance(this.world.getBallRef()) < this.world
				.getSelfRef().getPConf().PLAYER_SIZE
				+ SConf.getInstance().BALL_SIZE) {
			this.moveCollidedBallToPlayerMargin();
		}

		// if we didn't collide with the ball we collided with someone
		// find out who it was and set his speed to 0.
		if (!this.collidedWithBall && this.mightHaveCollidedWithPlayer) {
			int collidedPlayer = this.checkIfCollidedWithPlayerAfterVI();

			if (collidedPlayer >= 0) {
				Player p = this.world.getAllPlayers()[collidedPlayer];
				p.getSpeed().reset();
			}
		}
	}

	/**
	 * true if a reliable player was near enough that a collision with this
	 * player was possible.
	 */
	private boolean checkIfMightHaveCollidedWithPlayer() {

		for (Player p : this.world.getAllPlayers()) {
			if (!p.isMe()
					&& p.isReliable()
					&& this.world.getAge(p) == 1
					&& p.getDistance(this.expectedPosition) < p.getPConf().PLAYER_SIZE
							+ this.world.getSelfRef().getPConf().PLAYER_SIZE
							+ p.getPConf().PLAYER_SPEED_MAX) {
				return true;
			}
		}
		return false;
	}

	/**
	 * the ball is moved in radial direction outside the player.
	 */
	private void moveCollidedBallToPlayerMargin() {

		Ball ballRef = this.world.getBallRef();
		double angle = this.world.getSelfRef()
				.getAngleTo(ballRef.getPosition());

		ballRef.getPosition().pointAtPolar(
				this.world.getMyPConf().PLAYER_SIZE
						+ SConf.getInstance().BALL_SIZE + 0.001, angle);
		ballRef.getPosition().addToThis(this.world.getSelfRef().getPosition());
	}

	/**
	 * the method checks if a collision has occured. A collision has (probably)
	 * occured if our Speed direction is in the opposite as the predicted
	 * direction. If we are very slow noise might lead to wrond results.
	 * 
	 * @return true if collision seems probable
	 */
	private boolean checkIfCollided() {

		Vektor speedMe = this.world.getSelfRef().getSpeed();
		// the expected self (set in World before an BS-Update)
		Player predMe = this.world.getExpectedSelf();
		Vektor predSpeedMe;
		if (predMe != null)
			predSpeedMe = predMe.getSpeed();
		else {
			System.out.println(this.world.getCycle()
					+ " GOT NO ME PREDICTION! (MeModel)");
			// no speed was predicted -> not enough infos for deciding
			return false;
		}
		// the speed that would result after a collision
		Vektor speedAfterCollision = predSpeedMe.cloned();
		speedAfterCollision.mult(-0.1);

		// the players speed is like the speed he would have after a collision
		boolean speedChanged = speedMe.sub(speedAfterCollision).getLength() < 0.03;

		// the speed direction has changed (server mult. speed by -.1) when
		// collision
		boolean speedDirChanged = ((signum(predSpeedMe.x) != signum(speedMe.x) 
                                  || speedMe.x < 0.01) 
                                  && (signum(predSpeedMe.y) != signum(speedMe.y) 
                                      || speedMe.y < 0.01));

		// it is not noise that changed the speed
		boolean speedRelevant = (predSpeedMe.getLength() > 0.03);

		// the return value
		boolean collided = (speedChanged && speedDirChanged && speedRelevant);

		return collided;
	}

	/**
	 * This method checks if the ball was the cause of the collision. It's
	 * called only after BS-Infos.
	 * 
	 */
	public boolean checkIfCollidedWithBallAfterBS() {

		// only play_on
		// the ball can be inside a player in dead/standard situations (i.e.
		// kick off)
		if (this.world.getPlayMode() != PLAY_MODE.PLAY_ON
				|| this.world.isViBased()) {
			return false;
		}

		Ball ballRef = this.world.getBallRef();
		double secureDist = (this.mightHaveCollidedWithPlayer) ? 0.1 : 0.5;

		boolean ballNear = (this.expectedPosition.getDistance(ballRef
				.getPosition()) < SConf.getInstance().BALL_SIZE
				+ this.world.getMyPConf().PLAYER_SIZE + secureDist);

		// ball far -> no collsion
		if (!ballNear)
			return false;

		// ball near && collided -> collided with ball
		// (yes. we can't be sure here, but some risk always remains ;-))
		if (this.collided) {
			return true;
		} else {
			// if we are too slow the detection of a collision only by the
			// players
			// speed isn't safe. so if we are slow and the ball is inside us
			// assume
			// we collided with the ball.
			boolean ballInside = this.expectedPosition.getDistance(ballRef
					.getPosition()) < SConf.getInstance().BALL_SIZE
					+ this.world.getMyPConf().PLAYER_SIZE;
			boolean meToSlowToDetectCollision = this.world.getSelfRef()
					.getSpeed().getLength() < 0.02;

			if (ballInside && meToSlowToDetectCollision) {
				return true;
			}
		}
		return false;
	}

	/**
	 * After a VI the ball speed is (if not seen) calculated using last and
	 * current position (ballModel). If we collided we have to correct this
	 * speed. We assume that we collided with the ball if the ball is near and
	 * we collided with something.
	 * 
	 * @return true if collided with ball
	 */
	private boolean checkIfCollidedWithBallAfterVI() {

		Ball ballRef = this.world.getBallRef();
		double secureDistance = (this.collided) 
      ? ((this.mightHaveCollidedWithPlayer) ? 0.1
				: 0.15)
				: 0.05;

		if ((this.collided || this.world.getSelfRef().getSpeed().getLength() < 0.02)
				&& this.world.getAge(ballRef) == 0
				&& !ballRef.speedIsReliable()) {
			if (this.world.getSelfRef().getDistance(ballRef) < this.world
					.getSelfRef().getPConf().PLAYER_SIZE
					+ SConf.getInstance().BALL_SIZE + secureDistance) {
				return true;
			}
		}
		return false;
	}

	/**
	 * checks if we collided with an other player. this method returns true if
	 * we collided and someone is felt very near.
	 * 
	 * @return
	 */
	private int checkIfCollidedWithPlayerAfterVI() {
		if (!this.collided) {
			return -1;
		}

		double myRad = this.world.getSelfRef().getPConf().PLAYER_SIZE;

		for (int i = 0; i < this.world.getAllPlayers().length; i++) {
			Player p = this.world.getAllPlayers()[i];

			if (this.world.getNumber() == i + 1
					|| !p.isReliable()
					|| p.getAge(this.world.getBodyCycle(),
							UPDATE_MODE.SEEN_NOTEAM) != 0
					|| p.speedIsReliable()) {
				continue;
			}

			if (this.world.getSelfRef().getDistance(p) < myRad
					+ p.getPConf().PLAYER_SIZE + 0.1) {
				return i;
			}
		}

		return -2;
	}

	/*
	 * (non-Javadoc)
	 * 
	 * @see robocup.component.worldmodel.IModel#update(robocup.component.infotypes.RefereeInfo)
	 */
	public void update(RefereeInfo info) {
	}

	/*
	 * (non-Javadoc)
	 * 
	 * @see robocup.component.worldmodel.IModel#update(robocup.component.infotypes.PlayModeInfo)
	 */
	public void update(PlayModeInfo info) {
	}

	/*
	 * (non-Javadoc)
	 * 
	 * @see robocup.component.worldmodel.IModel#update(robocup.component.infotypes.PlayerSayInfo)
	 */
	public void update(PlayerSayInfo info) {
	}

	/**
	 * @return true if collided
	 */
	public boolean collided() {

		return this.collided;
	}
  

  /**
   * @return Returns the collidedWithBall.
   */
  public boolean collidedWithBall() {

    return this.collidedWithBall;
  }

}
